#ifndef COMMANDER_
#define COMMANDER_

#include "include/commander.h"

Commander::Commander(void){
  this->computeMaxRotation();
  this->computeMaxMag();

  delaying = false;

  for(int i = 0; i < 4; ++i){
    last_sent_wheels[2*i] = 249;
    last_sent_wheels[2*i + 1] = 1;
  }

  last_y = 0;

  mast_ele = 1500;
  mast_pan = 1500;
  
  // Pan
  last_sent_arm[0] = 249;
  last_sent_arm[1] = 1;
  
  // Ele
  last_sent_arm[2] = 206;
  last_sent_arm[3] = 1;

  // Elbow
  last_sent_arm[4] = 193;
  last_sent_arm[5] = 1;

  // Wrist
  last_sent_arm[6] = 255;
  last_sent_arm[7] = 76;
  
  // Grip
  last_sent_arm[8] = 249;
  last_sent_arm[9] = 1;
  
  // Twist
  last_sent_arm[10] = 129;
  last_sent_arm[11] = 1;
  
}

Command* Commander::getCommand(InputState* in_state, RoverState* rvr_state){
  Command* output = new Command();

  int error = 0;

  if(getDriveTrain(in_state, rvr_state, output) < 0){
    fprintf(stderr, "In Commander::getCommand, unable to get command for drive train.\n");
    ++error;
  }

  if(getArm(in_state, rvr_state, output) < 0){

  }
  
  if(getMast(in_state, rvr_state, output) < 0){
    
  }

  return output;
}

int Commander::getMast(InputState* in_state, RoverState* rvr_state, Command* output){
  int y = (int)in_state->getValue(JOY_TH_Y);
  int x = (int)in_state->getValue(JOY_TH_X);

  if((in_state->getValue(JOY_BUT_0) & JOYSTICK_BUTTON_4_MASK) == JOYSTICK_BUTTON_4_MASK){
    mast_pan = 1500;
    mast_ele = 1500;
  }

  if(x > 128){
    // Pan left
    mast_pan += 5;
  }else if(x < 128){
    // Pan right
    mast_pan -= 5;
  }

  if(y > 128){
    // Ele up
    mast_ele -= 10;
  }else if(y < 128){
    // Ele down
    mast_ele += 10;
  }

  if(mast_ele > 1850){
    mast_ele = 1850;
  }else if(mast_ele < 1010){
    mast_ele = 1010;
  }

  if(mast_pan > 1850){
    mast_pan = 1850;
  }else if(mast_pan < 1150){
    mast_pan = 1150;
  }

  int mast_pan_ms = mast_pan;
  int mast_ele_ms = mast_ele;
  int value0 = -1;
  int value1 = -1;
  mast_pan_ms -= 500;
  mast_pan_ms /= SERVO_MS_SCALE;
  splitToTwo(mast_pan_ms, &value0, &value1);
  output->setValue(MAST_PAN_MOTOR_0, (unsigned char)value0);
  output->setValue(MAST_PAN_MOTOR_1, (unsigned char)value1);

  mast_ele_ms -= 500;
  mast_ele_ms /= SERVO_MS_SCALE;
  splitToTwo(mast_ele_ms, &value0, &value1);
  output->setValue(MAST_ELE_MOTOR_0, (unsigned char)value0);
  output->setValue(MAST_ELE_MOTOR_1, (unsigned char)value1);
  return 0;
}

int Commander::getArm(InputState* in_state, RoverState* rvr_state, Command* output){

  // Test for enable
  if((in_state->getValue(BUTTONS) & 4) == 4){
    // Gripper
    int button0 = in_state->getValue(BUTTONS);
    int value = -1;
    if((button0 & 2) == 2){
      // Open
      value = 1500;
    }else{
      // Close
      value = 800;

    }
    int value0 = -1;
    int value1 = -1;
    value -= 500;
    value /= SERVO_MS_SCALE;
    splitToTwo(value, &value0, &value1);
    output->setValue(ARM_GRIP_MOTOR_0, (unsigned char)value0);
    output->setValue(ARM_GRIP_MOTOR_1, (unsigned char)value1);

    double ratio = PI/170;

    // Twist
    int twist_val = in_state->getValue(MINI_TWIST);
    double twist_out_angle_r = (twist_val - 95)* ratio;
    int twist_out_ms = twist_out_angle_r * (1800./PI) + 1500;
    if(twist_out_ms > 2400){
      twist_out_ms = 2400;
    }else if(twist_out_ms < 600){
      twist_out_ms = 600;
    }
    twist_out_ms -= 500;
    twist_out_ms /= SERVO_MS_SCALE;
    splitToTwo(twist_out_ms, &value0, &value1);
    output->setValue(ARM_TWIST_MOTOR_0, (unsigned char)value0);
    output->setValue(ARM_TWIST_MOTOR_1, (unsigned char)value1);

    //Wrist
    int wrist_val = in_state->getValue(MINI_WRIST);
    double wrist_out_angle_r = (wrist_val - 129) * ratio;
    int wrist_out_ms = wrist_out_angle_r * (600./PI) + 1500;
    if(wrist_out_ms > 1825){
      wrist_out_ms = 1825;
    }else if(wrist_out_ms < 1140){
      wrist_out_ms = 1140;
    }
    wrist_out_ms -= 500;
    wrist_out_ms /= SERVO_MS_SCALE;
    splitToTwo(wrist_out_ms, &value0, &value1);
    output->setValue(ARM_WRIST_MOTOR_0, (unsigned char)value0);
    output->setValue(ARM_WRIST_MOTOR_1, (unsigned char)value1);
  
    // Elbow 
    int elbow_val = in_state->getValue(MINI_ELBOW);
    double elbow_out_angle_r = (elbow_val - 187) * ratio;
    elbow_out_angle_r *= -1;
    int elbow_out_ms = elbow_out_angle_r * (600./PI) + 1500;

    // Account for droop
    int droop_elbow = cos(elbow_out_angle_r) * 40;
    if(elbow_out_angle_r > PI/2){
      droop_elbow *= -1;
    }
    elbow_out_ms += droop_elbow;

    if(elbow_out_ms > 1900){
      elbow_out_ms = 1900;
    }else if(elbow_out_ms < 1270){
      elbow_out_ms = 1270;
    }
    elbow_out_ms -= 500;
    elbow_out_ms /= SERVO_MS_SCALE;
    splitToTwo(elbow_out_ms, &value0, &value1);
    output->setValue(ARM_ELBOW_MOTOR_0, (unsigned char)value0);
    output->setValue(ARM_ELBOW_MOTOR_1, (unsigned char)value1);

    // Shoul ELE
    int shoul_ele_val = in_state->getValue(MINI_SHOUL_ELE);
    double shoul_ele_out_angle_r = (shoul_ele_val - 110) * ratio;
    shoul_ele_out_angle_r *= -1;
    int shoul_ele_out_ms = shoul_ele_out_angle_r * (600./PI) + 1500;

    // Account for droop
    int droop_shoul_ele = sin(shoul_ele_out_angle_r) * 50;
    if(shoul_ele_out_angle_r > 0){
      droop_shoul_ele *= -1;
    }
    shoul_ele_out_ms += droop_shoul_ele;

    if(shoul_ele_out_ms > 2000){
      shoul_ele_out_ms = 2000;
    }else if(shoul_ele_out_ms < 1330){
      shoul_ele_out_ms = 1330;
    }
    shoul_ele_out_ms -= 500;
    shoul_ele_out_ms /= SERVO_MS_SCALE;
    splitToTwo(shoul_ele_out_ms, &value0, &value1);
    output->setValue(ARM_SHOUL_ELE_MOTOR_0, (unsigned char)value0);
    output->setValue(ARM_SHOUL_ELE_MOTOR_1, (unsigned char)value1);

    // Shoul pan 
    int shoul_pan_val = in_state->getValue(MINI_SHOUL_PAN);
    double shoul_pan_out_angle_r = (shoul_pan_val - 119) * ratio;
    int shoul_pan_out_ms = shoul_pan_out_angle_r * (600./PI) + 1500;
    if(shoul_pan_out_ms > 1925){
      shoul_pan_out_ms = 1925;
    }else if(shoul_pan_out_ms < 1350){
      shoul_pan_out_ms = 1350;
    }
    shoul_pan_out_ms -= 500;
    shoul_pan_out_ms /= SERVO_MS_SCALE;
    splitToTwo(shoul_pan_out_ms, &value0, &value1);
    output->setValue(ARM_SHOUL_PAN_MOTOR_0, (unsigned char)value0);
    output->setValue(ARM_SHOUL_PAN_MOTOR_1, (unsigned char)value1);

    //    recordArm(output);
  }else{
    // Cheat like a bitch
    for(int i = 0; i < 12; ++i){
      Command_Component comp = (Command_Component)(12+i);
      output->setValue(comp, (unsigned char)last_sent_arm[i]);
    }



  }
  return 0;

}

void Commander::recordArm(Command* output){
  // Cheat like a bitch again
  for(int i = 0; i < 12; ++i){
    Command_Component comp = (Command_Component)(12 + i);
    last_sent_arm[i] = (int)output->getValue(comp);
  }
}


bool Commander::validWheelCommand(wheel_command* input){
  bool error = false;
  if(input->steering_value_0 <= 0 || input->steering_value_0 > 255){
    error = true;
    fprintf(stderr, "In Commander::validWheelCommand, steer_value_0 was %d!!!\n", 
	    input->steering_value_0);
  }
  if(input->steering_value_1 <= 0 || input->steering_value_1 > 255){
    error = true;
    fprintf(stderr, "In Commander::validWheelCommand, steer_value_1 was %d!!!\n",
	    input->steering_value_1);
  }
  if(input->drive_value <= 0 || input->drive_value > 255){
    error = true;
    fprintf(stderr, "In Commander::validWheelCommand, drive_value was %d!!!\n",
	    input->drive_value);
  }
  if(error){
    return false;
  }
  return true;
}

int Commander::getDriveTrain(InputState* in_state, RoverState* rvr_state, Command* output){
  int x = in_state->getValue(JOY_X) - JOYSTICK_MID;
  int y = in_state->getValue(JOY_Y) - JOYSTICK_MID;
  int z = in_state->getValue(JOY_TWI) - JOYSTICK_MID;

  printf("x: %d y: %d last_y: %d\n", x, y, last_y);

  int compare = last_y;
  int y_compare = y;
  if(last_y == 0){
    compare++;
  }
  if(y == 0){
    y_compare++;
  }
  if(abs(abs(y_compare)-abs(compare)) != abs(y_compare - compare)){
    // They have different signs
    // Check if out of deadband
    double distance = sqrt(x*x + y*y);
    if(!(distance < 10)){
      // Trap it now
      y = last_y;
    }else{
      last_y = y;
    }
  }else{
    last_y = y;
  }

  int throt = in_state->getValue(JOY_THR);
  double throt_percent = throt/255.;

  int num_error = 0;

  double fr_per = 1.0;
  double fl_per = 1.0;
  double rr_per = 1.0;
  double rl_per = 1.0;

  // Check for turbo
  int button = in_state->getValue(JOY_BUT_1);
  if((button & FR_TURBO_BUTTON) == FR_TURBO_BUTTON){
    fr_per *= TURBO_GAIN;
  }
  if((button & FL_TURBO_BUTTON) == FL_TURBO_BUTTON){
    fl_per *= TURBO_GAIN;
  }
  if((button & RR_TURBO_BUTTON) == RR_TURBO_BUTTON){
    rr_per *= TURBO_GAIN;
  }
  if((button & RL_TURBO_BUTTON) == RL_TURBO_BUTTON){
    rl_per *= TURBO_GAIN;
  }

  if(x < 10 && x > -10 && y < 10 && y > -10 && z < 10 && z > -10){
    for(int i = 0; i < 4; ++i){
      Command_Component comp = (Command_Component)(i);
      output->setValue(comp, (unsigned char)128);
    }
    
    // Make wheels sticky
    for(int i = 0;i < 8; ++i){
      Command_Component comp = (Command_Component)(4+i);
      output->setValue(comp, (unsigned char)last_sent_wheels[i]);
    }
  }else if(notInDeadBand(z, 0, JOY_TWI_DEADBAND)&& enabledTwist(in_state)){
    // We need rotation
    
    // Compute percent [-1.0, 1.0]
    double z_percent = ((double)z)/JOYSTICK_MID;
    double parellel_r = atan2(y, x);
    if(parellel_r < 0){
      parellel_r += PI;
    }

    // z is positive counter_clockwise
    double perp_r = -1;
    if(z > 0){
      perp_r = parellel_r + PI/2;
    }else{
      perp_r = parellel_r - PI/2;
    }

    double max_r = 128;
    double r = 128 * (1-fabs(z_percent)) + 17;

    double tar_y = sin(perp_r) * r;
    double tar_x = cos(perp_r) * r;
    if(x < 10 && x > -10 && y < 10 && y > -10){
      tar_x = 0;
      tar_y = 0;
    }
    printf("Tar_y: %f | Tar_x: %f\n", tar_y, tar_x);

    wheel_command fr, fl, rr, rl;

    // Find percentage based on maximume magitude of vecotr
    double mag = sqrt(x * x + y * y);
    double mag_per =  mag/this->max_mag;
    double percent = mag_per;
    if(x < 10 && x > -10 && y < 10 && y > -10){
      // Need to rotate in place use z_percent
      percent = fabs(z_percent);
    }

    double omega = calculateOmega(tar_x, tar_y, percent, throt_percent);
    
    getTwistValues(percent, z_percent, omega*fr_per, throt_percent, y,
		   FR_WHEEL_X, FR_WHEEL_Y, tar_x, tar_y, &fr);
    if(validWheelCommand(&fr)){
      limitPWM(&(fr.drive_value), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      output->setValue(FR_D_MOTOR, (unsigned char)fr.drive_value);
      output->setValue(FR_S_MOTOR_0, (unsigned char)fr.steering_value_0);
      output->setValue(FR_S_MOTOR_1, (unsigned char)fr.steering_value_1);
    }else{
      output->setValue(FR_D_MOTOR, (unsigned char)128);
      output->setValue(FR_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(FR_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);
      ++num_error;
      fprintf(stderr, "In Commander::getDriveTrain, twisting, fr invalid.\n");
    }

    getTwistValues(percent, z_percent, omega*fl_per, throt_percent, y,
		   FL_WHEEL_X, FL_WHEEL_Y, tar_x, tar_y, &fl);
    if(validWheelCommand(&fl)){
      limitPWM(&(fl.drive_value), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      output->setValue(FL_D_MOTOR, (unsigned char)fl.drive_value);
      output->setValue(FL_S_MOTOR_0, (unsigned char)fl.steering_value_0);
      output->setValue(FL_S_MOTOR_1, (unsigned char)fl.steering_value_1);
    }else{
      output->setValue(FL_D_MOTOR, (unsigned char)128);
      output->setValue(FL_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(FL_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);
      ++num_error;
      fprintf(stderr, "In Commander::getDriveTrain, twisting, fl invalid.\n");
    }

    getTwistValues(percent, z_percent, omega*rr_per, throt_percent, y,
		   RR_WHEEL_X, RR_WHEEL_Y, tar_x, tar_y, &rr);
    if(validWheelCommand(&rr)){
      limitPWM(&(rr.drive_value), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      output->setValue(RR_D_MOTOR, (unsigned char)rr.drive_value);
      output->setValue(RR_S_MOTOR_0, (unsigned char)rr.steering_value_0);
      output->setValue(RR_S_MOTOR_1, (unsigned char)rr.steering_value_1);
    }else{
      output->setValue(RR_D_MOTOR, (unsigned char)128);
      output->setValue(RR_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(RR_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);
      ++num_error;
      fprintf(stderr, "In Commander::getDriveTrain, twisting, rr invalid.\n");
    }

    getTwistValues(percent, z_percent, omega*rl_per, throt_percent, y,
		   RL_WHEEL_X, RL_WHEEL_Y, tar_x, tar_y, &rl);
    if(validWheelCommand(&rl)){
      limitPWM(&(rl.drive_value), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      output->setValue(RL_D_MOTOR, (unsigned char)rl.drive_value);
      output->setValue(RL_S_MOTOR_0, (unsigned char)rl.steering_value_0);
      output->setValue(RL_S_MOTOR_1, (unsigned char)rl.steering_value_1);
    }else{
      output->setValue(RL_D_MOTOR, (unsigned char)128);
      output->setValue(RL_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(RL_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);
      ++num_error;
      fprintf(stderr, "In Commander::getDriveTrain, twisting, rl invalid.\n");
    }
  }else{
    // We don't need rotation
    
    // All wheels are the same
    wheel_command command;
    getSimpleDrive(throt_percent, x, y, &command);
    if(validWheelCommand(&command)){

      int fr_val = ((command.drive_value - 128)*fr_per) + 128;
      int fl_val = ((command.drive_value - 128)*fl_per) + 128;
      int rr_val = ((command.drive_value - 128)*rr_per) + 128;
      int rl_val = ((command.drive_value - 128)*rl_per) + 128;
      
      limitPWM(&(fr_val), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      limitPWM(&(fl_val), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      limitPWM(&(rr_val), MOTOR_MAX_PWM, MOTOR_MIN_PWM);
      limitPWM(&(rl_val), MOTOR_MAX_PWM, MOTOR_MIN_PWM);


      // Set FR
      output->setValue(FR_D_MOTOR, (unsigned char)fr_val);
      output->setValue(FR_S_MOTOR_0, (unsigned char)command.steering_value_0);
      output->setValue(FR_S_MOTOR_1, (unsigned char)command.steering_value_1);

      // Set FL
      output->setValue(FL_D_MOTOR, (unsigned char)fl_val);
      output->setValue(FL_S_MOTOR_0, (unsigned char)command.steering_value_0);
      output->setValue(FL_S_MOTOR_1, (unsigned char)command.steering_value_1);

      // Set RR
      output->setValue(RR_D_MOTOR, (unsigned char)rr_val);
      output->setValue(RR_S_MOTOR_0, (unsigned char)command.steering_value_0);
      output->setValue(RR_S_MOTOR_1, (unsigned char)command.steering_value_1);

      // Set RL
      output->setValue(RL_D_MOTOR, (unsigned char)rl_val);
      output->setValue(RL_S_MOTOR_0, (unsigned char)command.steering_value_0);
      output->setValue(RL_S_MOTOR_1, (unsigned char)command.steering_value_1);
    }else{
      ++num_error;
      fprintf(stderr, "In Commander::getDriveTrain, simple drive, command invalid.\n");

      // Set FR
      output->setValue(FR_D_MOTOR, (unsigned char)128);
      output->setValue(FR_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(FR_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);

      // Set FL
      output->setValue(FL_D_MOTOR, (unsigned char)128);
      output->setValue(FL_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(FL_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);

      // Set RR
      output->setValue(RR_D_MOTOR, (unsigned char)128);
      output->setValue(RR_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(RR_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);

      // Set RL
      output->setValue(RL_D_MOTOR, (unsigned char)128);
      output->setValue(RL_S_MOTOR_0, (unsigned char)SERVO_STRAIGHT_0);
      output->setValue(RL_S_MOTOR_1, (unsigned char)SERVO_STRAIGHT_1);
    }
  }
  checkForWheelDelay(output);
  recordWheels(output);
  if(delaying){
    simulateSteeringDelay();
    delayWheels(output);
    checkForDelayEnd();
  }else{
    recordSimulatedWheels(output);
  }

  if(num_error > 0){
    fprintf(stderr, "In Commander::getDriveTrain. Ended with errors, count: %d\n", num_error);
    return -1;
  }
  return 0;
}

void Commander::simulateSteeringDelay(void){
  double rate = SERVO_SLEW_RATE;  // 150 ms per 0.8 seconds. 150 ms is PI/4
  struct timeval current;
  gettimeofday(&current, NULL);
  
  long seconds = current.tv_sec - delay_start.tv_sec;
  long useconds = current.tv_usec - delay_start.tv_usec;

  long mtime = ((seconds) * 1000 + useconds/1000.0) + 0.5;

  double percent = mtime/ms_delay;

  for(int i = 0; i < 4; ++i){
    double ms_diff = delay_end_wheel_ms[i] - delay_start_wheels_ms[i];
    double current_ms_diff = ms_diff * percent;
    simulated_wheel_ms[i] = current_ms_diff + delay_start_wheels_ms[i];
  }
}

void Commander::recordWheels(Command* output){
  for(int i = 0; i < 8; ++i){
    Command_Component comp = (Command_Component)(4+i);
    last_sent_wheels[i] = (int)output->getValue(comp);
  }
}

void Commander::recordSimulatedWheels(Command* output){
  for(int i = 0; i < 4; ++i){
    Command_Component comp_0 = (Command_Component)(4+(2*i));
    Command_Component comp_1 = (Command_Component)(4+(2*i + 1));
    int comp_0_int = (int)output->getValue(comp_0);
    int comp_1_int = (int)output->getValue(comp_1);
    double last_ms = ((comp_0_int + comp_1_int) * SERVO_MS_SCALE) + 500;
    simulated_wheel_ms[i] = last_ms;
  }
}

void Commander::checkForDelayEnd(void){
  struct timeval current;
  gettimeofday(&current, NULL);
  
  long seconds = current.tv_sec - delay_start.tv_sec;
  long useconds = current.tv_usec - delay_start.tv_usec;

  long mtime = ((seconds) * 1000 + useconds/1000.0) + 0.5;

  if(mtime > ms_delay){
    // Done delaying
    delaying = false;
  }
}

void Commander::checkForWheelDelay(Command* output){
  double rate = SERVO_SLEW_RATE;  // 150 ms per 0.8 ms. 150 ms is PI/4
  double current_sent_wheel_ms[4];
  for(int i = 0; i < 4; ++i){
    Command_Component comp_0 = (Command_Component)(4+(2*i));
    Command_Component comp_1 = (Command_Component)(4+(2*i + 1));
    int comp_0_int = (int)output->getValue(comp_0);
    int comp_1_int = (int)output->getValue(comp_1);
    double comp_ms = ((comp_0_int + comp_1_int) * SERVO_MS_SCALE) + 500;
    current_sent_wheel_ms[i] = comp_ms;
  }

  // If we are delaying check if the incoming has changed at all from the last final that cause the delay
  if(delaying){
    double total_diff = 0;
    for(int i = 0;i < 4; ++i){
      total_diff = fabs(current_sent_wheel_ms[i] - delay_end_wheel_ms[i]);
    }
    if(total_diff < 100){
      return;
    }
  }
  

  double max_diff = -1;
  for(int i = 0; i < 4; ++i){
    double diff = fabs(simulated_wheel_ms[i] - current_sent_wheel_ms[i]);
    if(diff > max_diff){
      max_diff = diff;
    }
  }

  double max_time = max_diff/rate;
  // Need a deadband
  if(max_time > 200){
      gettimeofday(&delay_start, NULL);
      ms_delay = max_time;
      delaying = true;

      for(int i = 0; i < 4; ++i){
	delay_start_wheels_ms[i] = simulated_wheel_ms[i];
	delay_end_wheel_ms[i] = current_sent_wheel_ms[i];
      }
  }
}

void Commander::delayWheels(Command* output){
  // I like cheating like a bitch
  for(int i = 0; i < 4; ++i){
    Command_Component comp = (Command_Component)(i);
    output->setValue(comp, (unsigned char)128);
  }
}

int Commander::getSimpleDrive(double throt_per, int x, int y, wheel_command* output){
  // In this drive mode. All wheel have the same angle
  
  // Compute theta in radians
  double theta_r = atan2(y, x);
  
  // Ensure theta is [0, pi] so servos can use
  if(theta_r < 0){
    theta_r += PI;
  }

  // Convert to an ms the arduino can understand
  int ms = radsToMS(theta_r);
  ms /= SERVO_MS_SCALE;

  // Get the two servo values
  int value_0, value_1;
  splitToTwo(ms, &value_0, &value_1);

  // Now get Wheel Speed
  int drive;
  drive = getSimpleWheelSpeed(throt_per, x, y);
  
  // Zero and assign values
  bzero((char*)output, sizeof(wheel_command));
  output->steering_value_0 = value_0;
  output->steering_value_1 = value_1;
  output->drive_value = drive;

  /*  if(x < 10 && x > -10 && y < 10 && y > -10){
    output->steering_value_0 = 249;
    output->steering_value_1 = 1;
    }*/

  return 0;
}

void Commander::limitPWM(int* value, int max, int min){
  if(*value > max){
    *value = max;
  }else if(*value < min){
    *value = min;
  }
}

int Commander::getSimpleWheelSpeed(double throt_per, int x, int y){
  // Compute magnitude of vector
  double mag = sqrt(x * x + y * y);

  // Find percentage based on maximume magitude of vecotr
  double mag_per =  mag/this->max_mag;

  // Find speed by simply proportion
  int speed = MAX_MOTOR_SPEED * mag_per * throt_per;

  // Negate if need to go opposite direction
  if(y < 0){
    speed *= -1;
  }

  // Get speed into [0, 255]
  int final_speed = speed + 128;

  // Insure nonzeo
  if(final_speed == 0){
    final_speed = 1;
  }

  return final_speed;
}


int Commander::getTwistValues(double percent, double z_percent, double omega, double throt_per,
			      double direction, int rover_x, int rover_y, int tar_x, int tar_y,
			      wheel_command* output){
  // Difference between target and wheel location
  int diff_x = tar_x - rover_x;
  int diff_y = tar_y - rover_y;

  // Angle of the parellel line in radians
  double parellel_r = atan2(diff_y, diff_x);

  // Insure the parellel angle is in [pi, 0]
  if(parellel_r < 0){
    parellel_r += PI;
  }

  // Find perpendicular angle in radians
  double perp_r = -1;
  if(parellel_r < PI/2){
    perp_r = parellel_r + PI/2;
  }else{
    perp_r = parellel_r - PI/2;
  }

  // Convert to ms [0, 2000] rover itself adds 500
  int ms = radsToMS(perp_r);

  // Convert to ms [0, 500] for transfering
  ms /= SERVO_MS_SCALE;

  // Get the two values
  int value_0, value_1;
  splitToTwo(ms, &value_0, &value_1);

  // Get drive [-MAX_MOTOR_ROTATE, MAX_MOTOR_ROTATE]
  int drive;
  drive = getTwistWheelSpeed(percent, z_percent, omega, throt_per, direction, diff_x, diff_y);

  // Transfrom drive [0, 255]
  drive += MOTOR_MIDPOINT;

  // Zero and assign values
  bzero((char*) output, sizeof(wheel_command));
  output->steering_value_0 = value_0;
  output->steering_value_1 = value_1;
  output->drive_value = drive;

  return 0;
}

double Commander::calculateOmega(double tar_x, double tar_y, double percent, double throt_per){
  double wheel_x[4];
  double wheel_y[4];

  wheel_x[0] = FR_WHEEL_X;
  wheel_y[0] = FR_WHEEL_Y;

  wheel_x[1] = FL_WHEEL_X;
  wheel_y[1] = FL_WHEEL_Y;

  wheel_x[2] = RR_WHEEL_X;
  wheel_y[2] = RR_WHEEL_Y;

  wheel_x[3] = RL_WHEEL_X;
  wheel_y[3] = RL_WHEEL_Y;

  double max_radius = -1;
  for(int i = 0; i < 4; ++i){
    double diff_x = wheel_x[i] - tar_x;
    double diff_y = wheel_y[i] - tar_y;

    double radius = sqrt(diff_x * diff_x + diff_y * diff_y);
    if(radius > max_radius){
      max_radius = radius;
    }
  }
  double speed = MAX_MOTOR_SPEED * percent * throt_per;
  double omega = speed/max_radius;
  // wr = v
  return omega;
}


int Commander::getTwistWheelSpeed(double percent, double z_percent, double omega,
				  double throt_per, double direction, int diff_x, int diff_y){
  // Determine if we need to negate the result
  int negate = 1;
  if(diff_x > 0){ //tar_x > rover_x
    negate *= -1;
  }
  if(z_percent < 0){
    negate *= -1;
  }
  if(direction < 0){
    negate *= -1;
  }

  // Find speed by simply proportion
  //  int speed = MAX_MOTOR_SPEED * percent;
  
  // Calculate radius
  double radius = sqrt(diff_x * diff_x + diff_y * diff_y);
  double speed = omega * radius;

  // Recall wr = v
  //  double result = percent * 32 * negate;
  //  double result =  (int)(omega * radius) * negate;
  return speed * negate;
}

void Commander::computeMaxRotation(void){
  // Calculate Max Rotation

  // Compute worst case radius
  int max_x = WORST_X + JOYSTICK_MAX_MOD_X;
  int max_y = WORST_Y + JOYSTICK_MAX_MOD_Y;
  double radius = sqrt(max_x * max_x + max_y * max_y);

  // Now compute max rotation
  double max_rotate = MAX_MOTOR_ROTATE/radius;

  this->max_rotate = max_rotate;
}

void Commander::splitToTwo(int input, int* value_0, int* value_1){
  *value_0 = input;
  *value_1 = input - MAX_VALUE_SPLIT;

  if(*value_1 <= 0){
    *value_1 = MIN_VALUE_SPLIT;
    if(*value_0 > MIN_VALUE_SPLIT){
      *value_0 -= MIN_VALUE_SPLIT;
    }
  }else{
    *value_0 = MAX_VALUE_SPLIT;
  }
}

int Commander::radsToMS(double rads){
  double start_point = SERVO_MS_180 - SERVO_MS_MIN;
  
  double ms_from_top = rads * SERVO_MS_DIFF/PI;

  int ms = SERVO_MS_DIFF - ms_from_top + start_point;

  return ms;
}

bool Commander::enabledTwist(InputState* in_state){
  int button0 = in_state->getValue(JOY_BUT_0);

  if((button0 & ENABLE_TWIST_MASK) == ENABLE_TWIST_MASK){
    return true;
  }
  return false;
}

bool Commander::notInDeadBand(int value, int center, int deadband){
  if(value > (center + deadband) || value < (center - deadband)){
    return true;
  }
  return false;
}

void Commander::computeMaxMag(void){
  double max = sqrt(JOYSTICK_MAX_MOD_X * JOYSTICK_MAX_MOD_Y + JOYSTICK_MAX_MOD_Y * JOYSTICK_MAX_MOD_Y);

  this->max_mag = max;
}


#endif
